COMMENT ⊗   VALID 00008 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	.SBTTL	User commands - feature tracking
C00006 00003	
C00008 00004	
C00010 00005	
C00012 00006	
C00014 00007	.SBTTL	subroutines for user command
C00018 00008	.SBTTL	variables and constants for user command
C00020 ENDMK
C⊗;
.SBTTL	User commands - feature tracking

;	This command tracks a set of features from one picture to
;	another using correlation.  It does not check numerical
;	parameters for validity. Its argument is a control block:

IDFPIC=0	;  ID of first picture
IDNPIC=2	;  ID of second picture
IDDAT=4		;  ID of data block containing set of features
WIDTH=6		;  width of window
WLENG=10	;  length of window
XLIM=12		;  X shift limit (positive)
YLIM=14		;  Y shift limit (positive)
SHFT=16		;  shift increment (+ or -)
CORDEB=20	;  non-zero for debugging each correlation
BESDEB=22	;  non-zero for debugging best correlation

;	It returns:
;		1  command number
;		2  zero - data block contains output
;		3  error bits decoded as follows:
;		       -2	best correlation debugging output
;		       -1	each correlation debugging output
;				(continues when command # is cleared)
;			0	ok  - finished with this pair of images
;		        1	control block not found
;			2	picture 1 does not exist
;			3	picture 2 does not exist
;			4	data block not found
;			5	BITS>4 for one of the pictures

;	The data block has the following format:
DCNT=0		; total number of features - set by 10
CURBLK=2	; number of feature last processed - set by 11
		; 	valid for debugging output only
		; this is followed by blocks of feature data as follows:
XFRST=0		; X coordinate of feature window in first image
YFRST=2		; Y    "   - these are set by 10
XSCND=4		; X coordinate of feature window in second image
YSCND=6		; Y    "   - these are set to XFRST,YFRST by 10 for first
		;	call and set to current values by 11 as it tracks
FEAT=10		; non-zero if active feature - set by 10 for first call
		;	and cleared by 11 for windows which went outside
BSCOR=12	; best correlation for latest image - set by 11
		;	for debugging this will be either latest or best
NSCOR=14	; second best correlation for latest image - set by 11
		; 	this is not set for correlation debugging
XINCR=16	; last X increment (for prediction)
YINCR=20	; last Y  " - these are cleared by 10 for first call
		; at end of command they contain coords. of 2nd best match
SBLK=YINCR+2	; size of feature block

;	pointer AC definitions

PIC=%3
CTL=%4
DAT=%5

COM5:	CLR OBUF+2
	MOV IBUF+2,-(SP)	;get control block in CTL
	JSR PC,SEARCH
	MOV (SP)+,CTL
	MOV CTL,PARAMS
	BNE PARFND
	MOV #1,OBUF+4		;	error 1 - not found
	JMP ERRRET
PARFND:	MOV IDDAT(CTL),-(SP)	;get data block in DAT
	JSR PC,SEARCH
	MOV (SP)+,DAT
	MOV DAT,DATA
	MOV DAT,DATCTL
	BNE DATFND
	MOV #4,OBUF+4		;	error 4 - not found
	JMP ERRRET
DATFND:	MOV IDFPIC(CTL),PIC	;get picture block 1
	MOV #3,OBUF+4
	JSR PC,PICFND
	MOV PIC,PICT1
	MOV IDNPIC(CTL),PIC	;get picture block 2
	MOV #2,OBUF+4
	JSR PC,PICFND
	MOV PIC,PICT2
	MOV WIDTH(CTL),PAR+6	;window width
	MOV WIDTH(CTL),B
	MUL WLENG(CTL),B
	MOV B,PAR+8		;window size
	MOV #1,CURBLK(DAT)	;initial tracking loop
	ADD #4,DATA		;DATA will point to start of current block
MANLOP:	MOV #RES1,RESADR	;start of main tracking loop
	MOV PICT1,PIC
	MOV DATA,DAT
	MOV XFRST(DAT),B
	MOV YFRST(DAT),C
	JSR PC,SETPAR		;set up parameters for first picture
	TST A
	BNE RES1
NOTINX:	CLR FEAT(DAT)		;window outside image - delete this feature
	JMP NXTBLK		;	and return for next feature

RES1:	MOV #PREFST,A		;wait for SPS ready
	JSR PC,DWELL
	JSR NEW,FSTWIN		;process first picture
	.WORD PAR/2
	MOV PARAMS,CTL		;restore pointers
	MOV DATA,DAT
	MOV #30000.,DIFF0
	MOV #30000.,DIFF1
	MOV #29000.,DSCOR
	MOV SHFT(CTL),YI	;init for Y loop
	ADD YINCR(DAT),YSCND(DAT)
	ADD XINCR(DAT),XSCND(DAT)
	MOV YSCND(DAT),YII
	MOV XSCND(DAT),XII
	CLR I
	CLR FY
	CLR DP
PASS1:	MOV SHFT(CTL),XI	;init for X loop
	MOV DP,DT
	MOV DP,J
	MOV DP,FX
	MOV XII,XSCND(DAT)
	ADD J,XSCND(DAT)
	MOV #30000.,LDIF
PASS2:	MOV XSCND(DAT),B
	MOV YSCND(DAT),C
	MOV PICT2,PIC		;get second picture pointer
	JSR PC,SETPAR		;and set up parameters
	TST A
	BEQ SKIPS		;window outside image - use dummy score
RES2:	MOV #PRENXT,A		;wait for SPS ready
	JSR PC,DWELL
	MOV #RES2,RESADR
	JSR NEW,NXTWIN		;correlate
	.WORD PAR/2
	MOV PARAMS,CTL
	MOV PICT2,PIC
	MOV DATA,DAT
	MOV #AFTNXT,A
	JSR PC,DWELL
SKIPS:	CMP SCORE,DIFF0		;update minimums
	BHIS NOLOW
	MOV DIFF0,DIFF1
	MOV DP,DPP
	MOV DL,DLL
	MOV SCORE,DIFF0
	MOV J,DP
	MOV I,DL
	BR NOLOW1

NOLOW:	CMP SCORE,DIFF1
	BHIS NOLOW1
	MOV SCORE,DIFF1
	MOV J,DPP
	MOV I,DLL

NOLOW1:	TST CORDEB(CTL)
	BEQ NODEB		;return debugging data if requested
	MOV J,A
	MOV I,B
	JSR PC,PAROUT
	MOV #-1,OBUF+4
	MOV SCORE,BSCOR(DAT)
	JSR PC,WAITL
NODEB:	CMP SCORE,LDIF		;update x loop
	BHI SCHI
	BEQ XEND
	MOV SCORE,LDIF		;new low for this line - save
	MOV J,DT
	BR XEND			; and check for end of line

SCHI:	CMP DT,FX		;higher score for this line
	BNE YUP			;low not first - end of line
	CMP XI,SHFT(CTL)	;low first - have we reversed
	BNE YUP			;yes - end of line
REVX:	NEG XI			;no - reverse direction
	MOV FX,J
	MOV XII,XSCND(DAT)
	ADD J,XSCND(DAT)
XEND:	ADD XI,J		;increment pointers
	ADD XI,XSCND(DAT)
	MOV J,A			;test for end of line
	BGE LL1
	NEG A
LL1:	CMP A,XLIM(CTL)
	BHI LLXXX		;not end  - continue
	JMP PASS2
LLXXX:	CMP XI,SHFT(CTL)	;end - reverse if possible
	BEQ REVX
YUP:	CMP LDIF,DIFF0		;update y loop
	BLOS YEND		;new low line score for correlation
	CMP DL,FY		;higher score
	BNE DONE		;low not first line - end of correlation
	CMP YI,SHFT(CTL)	;low first - have we reversed
	BNE DONE		;yes - end of correlation
REVY:	NEG YI			;no - reverse lines
	MOV FY,I
	MOV YII,YSCND(DAT)
	ADD I,YSCND(DAT)

YEND:	ADD YI,I		;test for end of correlation
	ADD YI,YSCND(DAT)
	MOV I,A
	BGE LL2
	NEG A
LL2:	CMP A,YLIM(CTL)
	BHI LL6			;not end - do next line
	JMP PASS1
LL6:	CMP YI,SHFT(CTL)
	BEQ REVY		;end - reverse if possible
DONE:	CMP #29000.,DIFF0
	BPL LL27		;no window in image
	JMP NOTINX
LL27:	MOV DP,A
	MOV DL,B
	JSR PC,PAROUT
	TST BESDEB(CTL)		;return best correlation if requested
	BEQ NXTBLK
	MOV #-2,OBUF+4
	JSR PC,WAITL
NXTBLK:	CMP #1,SHFT(CTL)
	BNE NXTB
	MOV DPP,XINCR(DAT)	;if last image, store coords. for 2nd best
	MOV DLL,YINCR(DAT)
	ADD XII,XINCR(DAT)
	ADD YII,YINCR(DAT)
NXTB:	MOV DATCTL,DAT
	INC CURBLK(DAT)		;update feature pointers
	ADD #SBLK,DATA
	CMP CURBLK(DAT),DCNT(DAT)
	BGT BLKDON
	JMP MANLOP
BLKDON:	CLR OBUF+4		;finished - return results
	JMP ERRRET

;	find picture data block and check bits
;	PIC contain ID on entry and pointer on exit

PICFND:	MOV PIC,-(SP)
	JSR PC,SEARCH
	MOV (SP)+,PIC
	BNE PICOK
PICBAD:	ADD #2,SP		;no block found - remove subr return
	JMP ERRRET		; and take error exit (flag already set)
PICOK:	CMP #4,BITS(PIC)
	BGE BITOK
	MOV #5,OBUF+4
	BR PICBAD
BITOK:	RTS PC
.SBTTL	subroutines for user command

;	set up parameters for picture pointed to by OLD
;	X,Y of starting point in B,C
;	A is zero on exit if window outside image

SETPAR:	MOV WRDLIN(PIC),PAR+4	;words/line
	SUB LEFT(PIC),B		;line increment in bytes for start
	BLT NOTIN
	MOV B,A
	ADD WIDTH(CTL),A
	CMP A,NSAMP(PIC)
	BLE BLKIN1
NOTIN:	INC DSCOR		;window not inside picture - set dummy score
	MOV DSCOR,SCORE
	CLR A			;	and flag
	RTS PC

BLKIN1:	CLR A
	DIV #4,A		;word increment and byte in last word
	MOV B,PAR+2		;byte number in word of start
	SUB TOP(PIC),C		;number of lines to start
	BLT NOTIN
	MOV C,B
	ADD WLENG(CTL),B
	CMP B,NLIN(PIC)
	BGT NOTIN
	MOV PIC,-(SP)
	MUL WRDLIN(PIC),C
	ADD LENG,A		;number of words to start
	ASL A			;convert to bytes
	MOV (SP)+,PIC
	ADD PIC,A		;address of picture block
	ADD PPNTR(PIC),A	;index in picture block of picture
	ASR A			;convert to word address
	MOV A,PAR		;word address of start of window
	RTS PC

; set up result block A,B are coords of best match

PAROUT:	MOV DIFF0,BSCOR(DAT)
	MOV DIFF1,NSCOR(DAT)
	MOV B,YINCR(DAT)
	MOV A,XINCR(DAT)
	MOV YII,YSCND(DAT)
	ADD B,YSCND(DAT)
	MOV XII,XSCND(DAT)
	ADD A,XSCND(DAT)
	RTS PC
.SBTTL	variables and constants for user command

;	wait for pdp-10 to pick up output

WAITL:	MOV COMAND,OBUF
WAILAB:	TST OBUF
	BNE WAILAB
	RTS PC

DIFF1:	0			;next to smallest score
DIFF0:	0			;smallest score
J:	0			;current X offset
I:	0			;current Y offset
XI:	0			;current X increment
YI:	0			;current Y increment
LDIF:	0			;smallest score for this line
DL:	0			;Y offset of smallest score for window
DP:	0			;X offset of smallest score for window
DLL:	0			;Y offset of 2nd smallest score
DPP:	0			;X offset of 2nd smallest score
DT:	0			;X offset of smallest score for current line
XII:	0			;initial X position
YII:	0			;initial Y position
FX:	0			;initial X offset
FY:	0			;initial Y offset
PICT1:	0			;pointer to first picture block
PICT2:	0			;pointer to second picture block
PARAMS:	0			;pointer to control block
DATA:	0			;pointer to current data block position
DATCTL:	0			;pointer to start of data block
DSCOR:	0			;dummy score if out of bounds

PREFST:
	.ASCIZ /BEFORE FSTWIN/

PRENXT:
	.ASCIZ /BEFORE NXTWIN/

AFTNXT:
	.ASCIZ /AFTER NXTWIN/

MONNAM:
	.ASCIZ \

HAND/EYE MONITOR - MON11C - VERSION OF 9/1/74
\

.EVEN